#include "common_inc.h"
#include "user_define.h"

extern bool InitPosFalg[7];
extern NEED_FIND_HOME_FLAG NeedFindHomeFlag;

#ifdef IMU6_T

#endif
// Used for response CAN message.
static CAN_TxHeaderTypeDef txHeader =
        {
                .StdId = 0,
                .ExtId = 0,
                .IDE = CAN_ID_STD,
                .RTR = CAN_RTR_DATA,
                .DLC = 8,
                .TransmitGlobalTime = DISABLE
        };

extern DummyRobot dummy;

extern COLOR_RGBC rgb;

#if defined(IMU6_T) || defined(IRC_T)
IMU6_SIGNAL imu6_data[5] ;
extern float IMU6_CompareRGB(uint16_t sr,uint16_t sg,uint16_t sb,uint16_t r,uint16_t g,uint16_t b);
float colorvalue = 0.0f;
extern uint16_t GetColorIndex;
uint8_t Can_ColorLock = 0;
float Color_Green[5],Color_Yellow[5],Color_White[5];
float ColorGreen,ColorYellow,ColorWhite;
extern void RGBtoHSL(COLOR_RGBC *Rgb, COLOR_HSL *Hsl);
COLOR_RGBC MyRGB[10],ColorRGB;
Sepan_RGBC MyRGB255;
static uint8_t lockcolorflg = 0;
#endif

void OnCanMessage(CAN_context *canCtx, CAN_RxHeaderTypeDef *rxHeader, uint8_t *data) {
    // Common CAN message callback, uses ID 32~0x7FF.
    if (canCtx->handle->Instance == CAN1) {
        uint8_t id = rxHeader->StdId >> 7; // 4Bits ID & 7Bits Msg
        uint8_t cmd = rxHeader->StdId & 0x7F; // 4Bits ID & 7Bits Msg

        //Respond(*usbStreamOutputPtr,"%c%c%c%c%c%c%c%c",data[0],data[1],data[2],data[3],data[4],data[5],data[6],data[7]);
        /*----------------------- �� Add Your CAN1 Packet Protocol Here �� ------------------------*/
        switch (cmd) {
            case 0x12:
                Respond(*usbStreamOutputPtr, "Id:%d CuLimit:%f A\n", id, *(float *) (data));
               // Respond(*uart1StreamOutputPtr, "Id:%d CuLimit:%f A\n", id, *(float *) (data));
                Respond(*uart4StreamOutputPtr, "Id:%d CuLimit:%f A\n", id, *(float *) (data));
                break;
            case 0x15:
                Respond(*usbStreamOutputPtr, "Id:%d set home pos to %d\n", id,*(int32_t*)data);
               // Respond(*uart1StreamOutputPtr, "Id:%d set home pos to %d\n", id,*(int32_t*)data);
                Respond(*uart4StreamOutputPtr, "Id:%d set home pos to %d\n", id,*(int32_t*)data);
                break;
            case 0x17:
                Respond(*usbStreamOutputPtr, "Id:%d P:%d\n", id, *(int32_t *) (data));
               // Respond(*uart1StreamOutputPtr, "Id:%d P:%d\n", id, *(int32_t *) (data));
                Respond(*uart4StreamOutputPtr, "Id:%d P:%d\n", id, *(int32_t *) (data));
                break;
            case 0x18:
                Respond(*usbStreamOutputPtr, "Id:%d V:%d\n", id, *(int32_t *) (data));
              //  Respond(*uart1StreamOutputPtr, "Id:%d V:%d\n", id, *(int32_t *) (data));
                Respond(*uart4StreamOutputPtr, "Id:%d V:%d\n", id, *(int32_t *) (data));
                break;
            case 0x19:
                Respond(*usbStreamOutputPtr, "Id:%d I:%d\n", id, *(int32_t *) (data));
              //  Respond(*uart1StreamOutputPtr, "Id:%d I:%d\n", id, *(int32_t *) (data));
                Respond(*uart4StreamOutputPtr, "Id:%d I:%d\n", id, *(int32_t *) (data));
                break;
            case 0x1A:
                Respond(*usbStreamOutputPtr, "Id:%d D:%d\n", id, *(int32_t *) (data));
               // Respond(*uart1StreamOutputPtr, "Id:%d D:%d\n", id, *(int32_t *) (data));
                Respond(*uart4StreamOutputPtr, "Id:%d D:%d\n", id, *(int32_t *) (data));
                break;
            case 0x23:
                if(id >=1 && id <=6 ){
                    dummy.motorJ[id]->UpdateAngleCallback(*(float *) (data), data[4]&0x01);
                    if(dummy.IS_NEED_FIND_HOME[id] != IS_NOT_NEED)
                        AdjustMagFlag[id] = (data[4]>>1)&0x01 ? true: false;
                    else
                        AdjustMagFlag[id] = true;

                    InitPosFalg[id] = (data[4]>>2)&0x01 ? true: false;
                    OverLoadFlag[id] = (data[4]>>3)&0x01 ? true: false;

                    if(OverLoadFlag[id]){
                        printf("Motor %d OverLoad\n");
                    }
                }
                #if defined(IMU6_T) || defined(IRC_T)
                imu6_data[id-1].data = *(float *) (data);

                    if(ReadPosFLG){
                        ReadPosFLG = 0;
                        //ZeroData[id -1] = imu6_data[id-1].data;
                        Respond(*usbStreamOutputPtr,"M1:%f,M2:%f,M3:%f,M4:%f,M5:%f\n", imu6_data[0].data, imu6_data[1].data, imu6_data[2].data,imu6_data[3].data,imu6_data[4].data);
                    }

                #endif
                break;
            case 0x31:
                Respond(*usbStreamOutputPtr, "Id:%d PIDV:%d,%d,%d,%d ok\n", id, *(int16_t *)&data[0],*(int16_t *)&data[2],*(int16_t *)&data[4],*(int16_t *)&data[6]);
                break;
            case 0x32:
                Respond(*usbStreamOutputPtr, "Motor:%d\n\tHard Ver:%d\n\tSoft Ver:%d.%d\n", id, data[0],data[3],data[4]);
                break;
            case 0x7B:
                rgb.r = *(unsigned short*)&data[0];
                rgb.g = *(unsigned short*)&data[2];
                rgb.b = *(unsigned short*)&data[4];
                rgb.c = *(unsigned short*)&data[6];
                //Respond(*usbStreamOutputPtr,"can rgbc %d %d %d %d\n",rgb.r,rgb.b,rgb.g,rgb.c);
                break;
            case 0x41:
                //if(id == 6)
                //    return;
                if(data[0])
                    InitPosFalg[id] = true;
                //if(FindHomeTimes != 0){
                    static int32_t HomeFlash;
                    static int32_t realPos;
                    static int32_t PosErr;
                    static int32_t AddOneCrile;;
                    static int32_t DecOneCrile;;
                    HomeFlash = *(uint16_t*)&data[2];
                    realPos = *(uint16_t*)&data[4];
                    PosErr = (realPos - HomeFlash);
                    AddOneCrile = realPos + (200*256 - HomeFlash);
                    DecOneCrile = (realPos - 200*256) - HomeFlash;
                    if(abs(PosErr) > abs(AddOneCrile))
                        PosErr = AddOneCrile;
                    if(abs(PosErr) > abs(DecOneCrile))
                        PosErr = DecOneCrile;
                    InitPosErr[id] = PosErr;
                    Respond(*usbStreamOutputPtr,"M%d find home as %5d (real:%5d) %5d\n",id,*(uint16_t*)&data[2],*(uint16_t*)&data[4],*(uint16_t*)&data[4]-*(uint16_t*)&data[2]);
                //}
                break;
            case 0x42:
                float ax,ay,az,MagDir;
                ax = ((float)*(short*)&data[0])/100;
                ay = ((float)*(short*)&data[2])/100;
                az = ((float)*(short*)&data[4])/100;
                MagDir = ((float)*(short*)&data[6])/100;
                //Respond(*usbStreamOutputPtr,"Id:%d ax:%.3f ay:%.3f az:%.3f MagDir:%.3f\n",id,ax,ay,az,MagDir);
                /*if(id ==1){
                    FloorImuData.ax = ax;
                    FloorImuData.ay = ay;
                    FloorImuData.az = az;
                    FloorImuData.MagDir = MagDir;
                }
                else */if(id == 4){
                    if(fabs(az) < 100){
                        MpuAngle[3] = az-FloorImuData.axAngle;
                    }

                }
                else if(id == 5){
                    if(fabs(az) < 100){
                        MpuAngle[4] = az+FloorImuData.ayAngle;
                    }

                }
                else if(id == 6){
                    if(fabs(az) < 100){
                        MpuAngle[5] = az+FloorImuData.axAngle;
                    }

                }
                else if(id == 7){
                    if(fabs(ay) < 100){
                        MpuAngle[6] = ay+FloorImuData.ayAngle;
                    }

                }
                break;
            case 0x43:
                if(data[0])
                    InitPosFalg[id] = true;
                if(FindHomeTimes != 0){
                    static uint16_t HomeFlash;
                    static uint16_t realPos;
                    static int32_t PosErr;
                    static int32_t AddOneCrile;;
                    static int32_t DecOneCrile;;
                    HomeFlash = *(uint16_t*)&data[2];
                    realPos = *(uint16_t*)&data[4];
                    PosErr = (realPos - HomeFlash);
                    AddOneCrile = realPos + (200*256 - HomeFlash);
                    DecOneCrile = (realPos - 200*256) - HomeFlash;
                    if(abs(PosErr) > abs(AddOneCrile))
                        PosErr = AddOneCrile;
                    if(abs(PosErr) > abs(DecOneCrile))
                        PosErr = DecOneCrile;

                    Respond(*usbStreamOutputPtr,"M%d find home as %5d (real:%5d) %5d\n",id,*(uint16_t*)&data[2],*(uint16_t*)&data[4],*(uint16_t*)&data[4]-*(uint16_t*)&data[2]);

                }

                break;
            case 0x44:
                TofDis = *(uint32_t*)&data[0];
                //Respond(*usbStreamOutputPtr,"TofDis:%d mm\n",TofDis);

                break;

            case 0x50:
#if defined(IMU6_T) || defined(IRC_T)
               // imu6_data[id-1].Find_Home = data[0];
                imu6_data[id-1].Elector1 = data[1];
                imu6_data[id-1].Elector2 = data[2];
                //imu6_data[id-1].Color_Flag = data[3];
//                if(id == 4){
//                    Respond(*usbStreamOutputPtr,"id:%d,Home:%d,single1:%d,signgle2:%d,single3:%d\n",id,data[0],data[1],data[2],data[3]);
//                }

#endif
                break;
            case 0x51:
#if defined(IMU6_T)

                imu6_data[id-1].rgb.r = data[0]<<8|data[1];
                imu6_data[id-1].rgb.g = data[2]<<8|data[3];
                imu6_data[id-1].rgb.b = data[4]<<8|data[5];
                imu6_data[id-1].rgb.c = data[6]<<8|data[7];


                if(FlyClass == 0){
                    MyRGB[GetColorIndex-1].r = imu6_data[id-1].rgb.r;
                    MyRGB[GetColorIndex-1].g = imu6_data[id-1].rgb.g;
                    MyRGB[GetColorIndex-1].b = imu6_data[id-1].rgb.b;

                    if(GetColorIndex == 5){
                        for(int i=0;i<5;i++){
                            ColorRGB.r += MyRGB[i].r;
                            ColorRGB.g += MyRGB[i].g;
                            ColorRGB.b += MyRGB[i].b;
                        }
                        ColorRGB.r/=5;
                        ColorRGB.g/=5;
                        ColorRGB.b/=5;
                        ColorGreen = IMU6_CompareRGB(345,2448,1107, ColorRGB.r, ColorRGB.g, ColorRGB.b);
                        ColorYellow = IMU6_CompareRGB(4823,1421,1000, ColorRGB.r, ColorRGB.g, ColorRGB.b);
                        Respond(*usbStreamOutputPtr,"Green:%f,Yellow:%f\n", ColorGreen,ColorYellow);
                        if(ColorGreen<1300 && ColorYellow > 2900){
                            imu6_data[id-1].Color_Flag = 2;
                        }
                        if(ColorYellow<3000 && ColorGreen>1500){
                            imu6_data[id-1].Color_Flag = 1;
                        }
                        GetColorIndex = 0;
                        ColorGreen = 0;
                        ColorYellow = 0;
                    }
                }
                else if(FlyClass == 1){

                    RGBto255RGB( &imu6_data[id-1].rgb,&MyRGB255);
                    Respond(*usbStreamOutputPtr,"r:%d,g:%d,b:%d\n",MyRGB255.r,MyRGB255.g,MyRGB255.b);
                    if((MyRGB255.r>MyRGB255.g&&MyRGB255.r>MyRGB255.b&&MyRGB255.g>MyRGB255.b && MyRGB255.r == 255) || (MyRGB255.b>MyRGB255.r  && MyRGB255.g>MyRGB255.r && MyRGB255.g>MyRGB255.b&&MyRGB255.g == 255)){

                        lockcolorflg = 1;

                    }
                    else if(MyRGB255.b == 255 || MyRGB255.g == 255){
                        if(lockcolorflg){
                            lockcolorflg = 0;
                            imu6_data[id-1].Color_Flag = 2;
                        }else if(lockcolorflg == 0){
                            imu6_data[id-1].Color_Flag = 1;
                        }

                    }
                }



//
//                Color_Green[GetColorIndex] = IMU6_CompareRGB(100,1000,100,imu6_data[2].rgb.r,imu6_data[2].rgb.g,imu6_data[2].rgb.b);
//                Color_Yellow[GetColorIndex] = IMU6_CompareRGB(1000,1000,100,imu6_data[2].rgb.r,imu6_data[2].rgb.g,imu6_data[2].rgb.b);
//
//
//                if(GetColorIndex == 5){
//                    for(int i=0;i<5;i++){
//                        ColorGreen+=Color_Green[i];
//                        ColorYellow+=Color_Yellow[i];
//                    }
//                    ColorGreen = ColorGreen/5;
//                    ColorYellow = ColorYellow/5;
//                    Respond(*usbStreamOutputPtr,"Green:%f,Yellow:%f,pei:%f\n", ColorGreen,ColorYellow,(ColorGreen)/(ColorGreen+ColorYellow));
////                    if(ColorGreen>=240&&ColorGreen<250){
////                        imu6_data[2].Color_Flag = 2;
////                    }
////                    else if(ColorGreen>=10&&ColorGreen<20){
////                        imu6_data[2].Color_Flag = 1;
////                    }
//                    GetColorIndex = 0;
//                }



#endif
                break;
            default:
                break;
        }

        dummy.UpdateJointAnglesCallback();

    } else if (canCtx->handle->Instance == CAN2) {
        /*----------------------- �� Add Your CAN2 Packet Protocol Here �� ------------------------*/
    }
    /*----------------------- �� Add Your Packet Protocol Here �� ------------------------*/
}